#pragma config(Hubs,  S2, HTMotor,  HTMotor,  HTServo,  none)
#pragma config(Sensor, S2,     ,               sensorI2CMuxController)
#pragma config(Motor,  mtr_S2_C1_1,     motorD,        tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S2_C1_2,     motorE,        tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S2_C2_1,     A,             tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S2_C2_2,     motorG,        tmotorTetrix, openLoop)
#pragma config(Servo,  srvo_S2_C3_1,    servo1,               tServoNone)
#pragma config(Servo,  srvo_S2_C3_2,    servo2,               tServoStandard)
#pragma config(Servo,  srvo_S2_C3_3,    servo3,               tServoNone)
#pragma config(Servo,  srvo_S2_C3_4,    servo4,               tServoNone)
#pragma config(Servo,  srvo_S2_C3_5,    servo5,               tServoNone)
#pragma config(Servo,  srvo_S2_C3_6,    servo6,               tServoNone)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#include "JoystickDriver.c"
#pragma DebuggerWindows("joystickSimple")
#pragma DebuggerWindows("debugStream")

int motorSmoother(int before, int after){
		int nSmooth = 1;
		if(before < after)
		{
		 after = before + nSmooth;
		}
	if(before > after){
		after = before - nSmooth;
	}
	else if(before == after)
	{
		return after;
	}

	return after;
 }


task teleop()
{
	bFloatDuringInactiveMotorPWM = false;
	int prev, val;
	bool stopped = false;
	while(true)
	{
	 getJoystickSettings(joystick);
	 /*if(abs(joystick.joy1_y1) > 20)
	 {
	  motor[A] = -joystick.joy1_y1;
	 }
	 if(abs(joystick.joy1_y1) < 20)
	 {
	   motor[A] = 0;
	 }*/
	 if(joy1Btn(2) == 1)
    {
  	  val= -100;
  	  ClearTimer(T1);
  	  stopped = false;

  	}
  	if(joy1Btn(3) == 1 && time10[T1] > 175)
  	{
  		val = 10;
  		stopped = false;
  	}
  	if(joy1Btn(3) == 0 && joy1Btn(2) == 0)
  	{
  		val = 0;
  		ClearTimer(T2);
  		stopped = true;
  	}

  	 writeDebugStreamLine("loop time = %d, motor[A] = %d, stop = %s", time1[T2], motor[motorD], stopped ? "True" : "False" );

  	prev = motor[motorD];
  	motor[motorE] = motorSmoother(prev, val);
  	wait10Msec(2);
  	if(joy1Btn(5) == 1)
  	{
  		servoTarget[srvo_S2_C3_2] = 0;
  	}
  	if(joy1Btn(6) == 1)
  		servoTarget[srvo_S2_C3_2] = 175;
  if(joy1Btn(7) == 1)
  	servoTarget[servo1] = 245;
 if(joy1Btn(8) == 1)
   	servoTarget[servo1] = 50;






  }
}

task main()
{
StartTask(teleop);
while(true)
{
	wait1Msec(1);
}

}
